#include "AReVi/arObject.h"
#include "AReVi/scheduler.h"
#include "AReVi/Lib3D/base3D.h"
#include "AReVi/Utils/abstractStream.h"
#include "AReVi/Contrib/xmlParser.h"
using namespace AReVi;

#include "awesomeMan.h"

AR_CLASS_DEF(AwesomeMan, Element)

StlVector<AwesomeMan::AnimationData *> AwesomeMan::_animations;
StlVector<AwesomeMan::ActionData *> AwesomeMan::_actions;
StlVector<AwesomeMan::StateData *> AwesomeMan::_states;
StlVector<AwesomeMan::TransitionData *> AwesomeMan::_transitions;

/******************************
 * Construction / Destruction *
 ******************************/

AwesomeMan::AwesomeMan(ArCW & arCW) : Element(arCW) {
    _acceleration = _speed = 0.0;
    _speedMin = -numeric_limits<double>::max();
    _speedMax = numeric_limits<double>::max();
}

AwesomeMan::~AwesomeMan() {
}

void AwesomeMan::init() {
    // Chargement du modèle
    StlString str = "./models/AwesomeMan/AwesomeMan.md5mesh";
    LoaderData data = HLibLoader::loadFile(str);
    if(data.failed) {
        cerr << "error: can't open " << str << endl;
    }
    _representation = data.body;
    _representation->accessBodyShape()->setShowSkeleton(false);
    _representation->translate(0.0,0.0,3.0);
    _animationOffset = 0.0;
}

/**********************************
 * Gestion accélération / vitesse *
 **********************************/

double AwesomeMan::getAcceleration() {
    return _acceleration;
}
void AwesomeMan::setAcceleration(double acc) {
    _acceleration = acc;
    fireUpdateLocation();
}

double AwesomeMan::getSpeed() {
    return _speed;
}
void AwesomeMan::setSpeed(double speed) {
    _speed = speed;
    fireUpdateLocation();
}

double AwesomeMan::getTurnCoeff() {
    return _turnCoeff;
}
void AwesomeMan::setTurnCoeff(double coeff) {
    _turnCoeff = clamp(coeff, -1.0, 1.0);
    fireUpdateLocation();
}

void AwesomeMan::setSpeedLimits(double & min, double & max) {
    min = _speedMin;
    max = _speedMax;
}
void AwesomeMan::setSpeedLimits(double min, double max) {
    _speedMin = min;
    _speedMax = max;
}

/*********************************************
 * Sérialisation pour gestion des évènements *
 *********************************************/

void AwesomeMan::writeConfiguration(ArRef<AbstractOStream>) {
}
void AwesomeMan::readConfiguration(ArRef<AbstractIStream>) {
}



void AwesomeMan::writeUpdate(ArRef<AbstractOStream>) {
}
void AwesomeMan::readUpdate(ArRef<AbstractIStream>) {
}

void AwesomeMan::writeLocation(ArRef<AbstractOStream> stream) {
    double x, y, z;
    getPosition(x, y, z);
    stream->writeDouble(x);
    stream->writeDouble(y);
    stream->writeDouble(z);
    extractOrientation(x, y, z);
    stream->writeDouble(x);
    stream->writeDouble(y);
    stream->writeDouble(z);
    stream->writeDouble(_acceleration);
    stream->writeDouble(_speed);
    stream->writeDouble(_speedMin);
    stream->writeDouble(_speedMax);
    stream->writeDouble(_turnCoeff);
}

void AwesomeMan::readLocation(ArRef<AbstractIStream> stream) {
	double x, y, z;
    stream->readDouble(x);
    stream->readDouble(y);
    stream->readDouble(z);
    setPosition(x, y, z);
    stream->readDouble(x);
    stream->readDouble(y);
    stream->readDouble(z);
    setOrientation(x, y, z);
    stream->readDouble(_acceleration);
    stream->readDouble(_speed);
    stream->readDouble(_speedMin);
    stream->readDouble(_speedMax);
    stream->readDouble(_turnCoeff);
}


/***************************************************************
 * Mise à jour de la position en fonction de l'action en cours *
 ***************************************************************/

void AwesomeMan::updateLocation() {
	double time = Scheduler::get()->getTime();
	double dt = time - _lastLocationUpdate;
	_lastLocationUpdate = time;

	_speed += _acceleration * dt;
	_speed = clamp(_speed, _speedMin, _speedMax);

    // S'il n'y a pas d'animation en cours, on ne fait rien
    if (_actionStack.size() == 0) return;

    // Sinon on avance ...
    AwesomeMan::ActionData * action = _actionStack[0];
	_animationOffset += _speed * dt;

    // Si jamais on est sorti de l'action courante
    if (_animationOffset > action->end->time) {
    	// On doit passer à la suite
    	if (_actionStack.size() > 1) {
    	    while (_animationOffset > action->end->time) _animationOffset -= action->end->time - action->begin->time;
     	    // ... soit à l'action suivante
            double x, y, z, x2, y2, z2;
            action->animation->animation->evaluate(action->end->time)->findJoint("Root")->getPosition(x2, y2, z2);
    	    _animationOffset -= action->begin->time;    	    
    	    _actionStack.erase(_actionStack.begin());

    	    action = _actionStack[0];
    	    _animationOffset += action->begin->time;
    	    
            action->animation->animation->evaluate(action->begin->time)->findJoint("Root")->getPosition(x, y, z);
            _representation->translate(x2-x, y2-y, z2-z);
         }
        else {
            if (! action->cyclic) {
                // ... soit on s'arrête
                _animationOffset = action->end->time;
                _actionStack.erase(_actionStack.begin());
            }
            else {
                // ... sinon on boucle
            	while (_animationOffset > action->end->time) {
                    double x, y, z, x2, y2, z2;
                    action->animation->animation->evaluate(action->begin->time)->findJoint("Root")->getPosition(x, y, z);
                    action->animation->animation->evaluate(action->end->time)->findJoint("Root")->getPosition(x2, y2, z2);
                    _representation->translate(x2-x, y2-y, z2-z);
                    _animationOffset -= action->end->time - action->begin->time;
            	}
            }
        }
    }

    _representation->applyPose(action->animation->animation->evaluate(min(_animationOffset,action->end->time)));
    

    ArRef<Base3D> baseNow = Base3D::NEW();
    ArRef<Base3D> baseAfter = Base3D::NEW();
    baseNow->setLocation(_representation->getSkeleton()->findJoint("Root"));
    baseAfter->setLocation(baseNow);
    double vx = 0.0, vy = 0.0, vz = 1.0;
    baseNow->globalToLocalVector(vx, vy, vz);
	baseAfter->rotate(vx,vy,vz, M_PI / 20.0 * _speed * _turnCoeff);
    _representation->move(baseAfter);
    baseNow->invertLocation();
    _representation->move(baseNow);
}


/***************************************
 * Chargement des animations / actions *
 ***************************************/

void AwesomeMan::loadAnimationAndActionData(StlString filename) {
cerr << "DEBUG: Loading animations ..." << endl;
    ArRef<XmlParser> parser = XmlParser::NEW();
    if (! parser->parseFile(filename) || ! parser->getRoot()) {
        cerr << "internal: Bad xml [" << filename << "]" << endl;
        return;
    }
    ArRef<XmlNode> root = parser->getRoot();
    // On va d'abord charger les animations
    ArRef<XmlNode> anims = root->getChild("Animations");
    if (! anims) {
        cerr << "error: Xml as no animation node" << endl;
        return;
    }
    ArRef<XmlNode> anim = anims->getFirstChild();
    while (anim) {
cerr << "DEBUG: - New animation ..." << endl;
        // D'abord on charge l'animation en elle même
        AwesomeMan::AnimationData * tmp = new AwesomeMan::AnimationData();
        StlString filename;
        if (! anim->getPropertyString("filename", filename) || ! anim->getPropertyString("name", tmp->name)) {
            cerr << "error: missing name or filename for animation" << endl;
            anim = anim->getNext();
            continue;
        }
        
        LoaderData data = HLibLoader::loadFile(filename);
        if(data.failed) {
            cerr << "error: can't open " << filename << endl;
            anim = anim->getNext();
            continue;
        }
        tmp->animation = data.animation;
        
        // Puis les données sur les keyframes
        ArRef<XmlNode> node = anim->getFirstChild();
        while (node) {
            StlString name, pose;
            int frame;
            double time;
            if (! node->getPropertyString("id", name) || ! node->getPropertyString("pose", pose)) {
                node = node->getNext();
                continue;
            }
            if (! node->getPropertyReal("time", time)) {
                if (! node->getPropertyInteger("frame", frame)) {
                    node = node->getNext();
                    continue;
                }
                if (frame < 0 || frame > (int)tmp->animation->getNbFrames()) {
                    time = tmp->animation->getDuration();
                }
                else {
                    time = 0.0;
                    for (int i = 0; i < frame; i++) {
                        time += ar_down_cast<KeyframeAnimation>(tmp->animation)->getFrameDuration(i);
                    }
                }
            }
            else if (time < 0.0) {
                time = tmp->animation->getDuration();
            }
            AwesomeMan::KeyData * key = new AwesomeMan::KeyData();
            key->time = time;
            key->name = name;
            key->pose = pose;
            tmp->keys.push_back(key);
            node = node->getNext();
        }
        AwesomeMan::_animations.push_back(tmp);
cerr << "DEBUG: loaded animation [" << tmp->name << "] with " << tmp->keys.size() << " keys" << endl;
        anim = anim->getNext();
    }


    // Ensuite on charge les actions
    ArRef<XmlNode> actions = root->getChild("Actions");
    if (! actions) {
        cerr << "error: Xml hasn't <Actions> node" << endl;
        return;
    }
    ArRef<XmlNode> action = actions->getFirstChild();
    while (action) {
cerr << "DEBUG: - loading action ..." << endl;
        // ... d'abord on récupère les propriétés
        StlString name, animation, start, end;
        bool cyclic = false;
        
        if (! action->getPropertyString("name", name) || ! action->getPropertyString("animation", animation) || ! action->getPropertyString("start", start) || ! action->getPropertyString("end", end)) {
            cerr << "error: Action node must have properties : name, animation, start and end" << endl;
            action = action->getNext();
            continue;
        }
        action->getPropertyBoolean("cyclic", cyclic);
        
        // ... ensuite on traite les données
        AwesomeMan::AnimationData * animPtr = NULL;
        AwesomeMan::KeyData * kbPtr = NULL, * kePtr = NULL;

        StlVector<AwesomeMan::AnimationData *>::iterator it = AwesomeMan::_animations.begin();
        while (it != AwesomeMan::_animations.end() && animPtr == NULL) {
            if ((*it)->name == animation) animPtr = *it;
            it++;
        }
        if (! animPtr) {
            cerr << "error: can't find animation " << animation << endl;
            action = action->getNext();
            continue;
        }

        StlVector<AwesomeMan::KeyData *>::iterator it2 = animPtr->keys.begin();
        while (it2 != animPtr->keys.end() && (kbPtr == NULL || kePtr == NULL)) {
            if ((*it2)->name == start) kbPtr = *it2;
            if ((*it2)->name == end) kePtr = *it2;
            it2++;
        }
        if (! kbPtr || ! kePtr) {
            cerr << "error: can't find keyframe named " << start << " or " << end << endl;
            action = action->getNext();
            continue;
        }

        AwesomeMan::ActionData * act = new AwesomeMan::ActionData();
        act->name = name;
        act->animation = animPtr;
        act->begin = kbPtr;
        act->end = kePtr;
        act->cyclic = cyclic;
        
        AwesomeMan::_actions.push_back(act);        

        action = action->getNext();
    }
    cerr << "DEBUG: found " << AwesomeMan::_actions.size() << " actions" << endl;
    
    // Chargement des différents états
    ArRef<XmlNode> states = root->getChild("States");
    if (! states) {
        cerr << "error: Xml hasn't <States> node" << endl;
        return;
    }
    ArRef<XmlNode> state = states->getFirstChild();
    while (state) {
cerr << "DEBUG: - loading state ..." << endl;
        // ... d'abord on récupère les propriétés
        StlString name;
        bool cyclic = false; // faux par défaut
        if (! state->getPropertyString("name", name)) {
            cerr << "error: No name for state" << endl;
            state = state->getNext();
            continue;
        }
        state->getPropertyBoolean("cyclic", cyclic);
        
        AwesomeMan::StateData * tmp = new AwesomeMan::StateData();
        tmp->name = name;
        tmp->cyclic = cyclic;
        
        // ... ensuite on récupère toutes les actions à l'intérieur
        ArRef<XmlNode> node = state->getFirstChild();
        while (node) {
            StlString actionName;
            if (node->getName() != "Action" || !node->getPropertyString("name", actionName)) {
                node = node->getNext();
                continue;
            }
            AwesomeMan::ActionData *actionPtr;
            StlVector<AwesomeMan::ActionData *>::iterator it = AwesomeMan::_actions.begin();
            while (it != AwesomeMan::_actions.end() && actionPtr == NULL) {
                if ((*it)->name == actionName) actionPtr = *it;
                it++;
            }
            if (! actionPtr) {
                cerr << "error: can't find action " << actionName << endl;
                node = node->getNext();
                continue;
            }
            tmp->actions.push_back(actionPtr);
            
            node = node->getNext();
        }
        cerr << "DEBUG:  - With " << tmp->actions.size() << " actions" << endl;
        AwesomeMan::_states.push_back(tmp);
        
        state = state->getNext();
    }
    cerr << "DEBUG: found " << _states.size() << " states" << endl;

    // Chargement des transitions
    ArRef<XmlNode> transitions = root->getChild("Transitions");
    if (! transitions) {
        cerr << "error: Xml hasn't <Transitions> node" << endl;
        return;
    }
    ArRef<XmlNode> transition = transitions->getFirstChild();
    while (transition) {
cerr << "DEBUG: - loading transition ..." << endl;
        // ... d'abord on récupère les propriétés
        StlString from, to;
        if (!transition->getPropertyString("from", from) || !transition->getPropertyString("to", to)) {
            cerr << "error: Transition must have properties from and to" << endl;
            transition = transition->getNext();
            continue;
        }
        
        // On cherche les liaisons
        AwesomeMan::StateData * fromState = NULL, * toState = NULL;
        StlVector<AwesomeMan::StateData *>::iterator it = AwesomeMan::_states.begin();
        while (it != AwesomeMan::_states.end() && (fromState == NULL || toState == NULL)) {
            if ((*it)->name == from) fromState = *it;
            if ((*it)->name == to) toState = *it;
            it++;
        }
        if (fromState == toState || fromState == NULL || toState == NULL) {
            cerr << "error: Error in transition" << endl;
            transition = transition->getNext();
            continue;
        }

        AwesomeMan::TransitionData * tmp = new AwesomeMan::TransitionData();
        tmp->to = toState;
        
        fromState->transitions.push_back(tmp);
        
        // On cherche les actions
        ArRef<XmlNode> node = transition->getFirstChild();
        while (node) {
            StlString actionName;
            if (node->getName() != "Action" || !node->getPropertyString("name", actionName)) {
                node = node->getNext();
                continue;
            }
            AwesomeMan::ActionData *actionPtr;
            StlVector<AwesomeMan::ActionData *>::iterator it = AwesomeMan::_actions.begin();
            while (it != AwesomeMan::_actions.end() && actionPtr == NULL) {
                if ((*it)->name == actionName) actionPtr = *it;
                it++;
            }
            if (! actionPtr) {
                cerr << "error: can't find action " << actionName << endl;
                node = node->getNext();
                continue;
            }
            tmp->actions.push_back(actionPtr);
            
            node = node->getNext();
        }
        cerr << "DEBUG:  - With " << tmp->actions.size() << " actions" << endl;
        _transitions.push_back(tmp);
        
        transition = transition->getNext();
    }
    cerr << "DEBUG: found " << _transitions.size() << " transitions" << endl;

}


/***********************
 * Gestion des actions *
 ***********************/

StlVector<StlString> AwesomeMan::getActions() {
    StlVector<StlString> res;
    for (unsigned int i = 0; i < AwesomeMan::_actions.size(); i++) {
        res.push_back(AwesomeMan::_actions[i]->name);
    }
    return res;
}

StlString AwesomeMan::getAction() {
    if (_actionStack.size() == 0) {
        return "";
    }
    else {
        return _actionStack[0]->name;
    }
}

StlString AwesomeMan::getLastAction() {
    if (_actionStack.size() == 0) {
        return "";
    }
    else {
        return _actionStack[_actionStack.size()-1]->name;
    }
}

void AwesomeMan::doAction(StlString actionName) {
    AwesomeMan::ActionData * action = NULL;
    StlVector<AwesomeMan::ActionData *>::iterator it = _actions.begin();
    while (it != _actions.end() && action == NULL) {
        if ((*it)->name == actionName) action = *it;
        it++;
    }
    if (action == NULL) {
        cerr << "internal: Bad action " << action << endl; 
        return;
    }
    if (_actionStack.size() == 0) {
        // On doit initialiser l'offset
        double x, y, z, x2, y2, z2;
        _representation->getSkeleton()->findJoint("Root")->getPosition(x2, y2, z2);
        _animationOffset = action->begin->time;
        action->animation->animation->evaluate(_animationOffset)->findJoint("Root")->getPosition(x, y, z);
        _representation->translate(x2-x, y2-y, z2-z);
        
        // On défini la vitesse à 1
        setAcceleration(0.0);
        setSpeed(1.0);
    }
    _actionStack.push_back(action);
}

